﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Matrix = Microsoft.Xna.Framework.Matrix;
using Vector = Microsoft.Research.Kinect.Nui.Vector;
using Vector3 = Microsoft.Xna.Framework.Vector3;

namespace Kinect.GPart
{
    class G3D
    {
        public static Vector getPlane(List<Vector3> vectors)
        {
            Matrix t1 = new Matrix();
            Vector3 t2 = new Vector3();
            foreach (Vector3 v in vectors)
            {
                t1.M11 += (v.X * v.X);
                t1.M12 += (v.X * v.Y);
                t1.M13 += (v.X);
                t1.M21 += (v.X * v.Y);
                t1.M22 += (v.Y * v.Y);
                t1.M23 += (v.Y);
                t1.M31 += (v.X);
                t1.M32 += (v.Y);
                t2.X += (v.X * v.Z);
                t2.Y += (v.Y * v.Z);
                t2.Z += (v.Z);
            }
            t1.M33 = vectors.Count;
            t1.M44 = 1;
            Matrix t4 = Matrix.Invert(t1);
            Vector3 t3 = Vector3.Transform(t2, t4);
            Vector result = new Vector();
            result.X = t3.X;
            result.Y = t3.Y;
            result.Z = -1;
            result.W = t3.Z;
            return result;
        }

        public static float dist2plane(Vector plane, Vector3 pt)
        {
            float t = Math.Abs(pt.X * plane.X + pt.Y * plane.Y + pt.Z * plane.Z + plane.W);
            return t / (float)Math.Sqrt(plane.X * plane.X + plane.Y * plane.Y + plane.Z * plane.Z);
        }

        ////计算方差
        //public static float getVariance(List<Vector3> vectors, Vector plane)
        //{
        //    float sum = 0;
        //    foreach (Vector3 v in vectors)
        //    {
        //        sum += (float)Math.Pow((plane.W + plane.X * v.X + plane.Y * v.Y + plane.Y * v.Y), 2);
        //    }
        //    sum /= vectors.Count();
        //    return sum;
        //}

    }
}
